#pragma config(Hubs,  S1, HTMotor,  HTServo,  HTMotor,  none)
#pragma config(Hubs,  S4, HTMotor,  HTMotor,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     gyroSensor,     sensorI2CHiTechnicGyro)
#pragma config(Sensor, S3,     SMUX,           sensorI2CCustom9V)
#pragma config(Sensor, S4,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     mtrFL,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     mtrBL,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     collector,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     motorG,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C1_1,     mtrBR,         tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S4_C1_2,     mtrFR,         tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S4_C2_1,     mtrLifterL,    tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S4_C2_2,     mtrLifterR,    tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Servo,  srvo_S1_C2_1,    goalGrabber,          tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    container,            tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "../../drivers/hitechnic-sensormux.h"
#include "../../drivers/lego-touch.h"
#include "../../drivers/hitechnic-irseeker-v2.h"

const tMUXSensor touchSensor = msensor_S3_1;
const tMUXSensor irSensor = msensor_S3_2;

#include "../include_files/functions.c"

task main()
{
	while(true)
	{
		//Right arrow to raise lift
  	if(nNxtButtonPressed == 1)
  	{
  		while(nNxtButtonPressed == 1)
  		{
  			RaiseLift();
  		}
  		StopLift();
  	}
  	//Left arrow to lower lift
  	if(nNxtButtonPressed == 2)
		{
			LowerLift();
			while(nNxtButtonPressed == 2)
			{
			LowerLift();
			}
			StopLift();
		}
		//Center button to run collector
		if(nNxtButtonPressed == 3)
		{
			while(nNxtButtonPressed == 3)
			{
				CollectBalls();
			}
			StopCollector();
		}
	}
}
